{
  "cells": [
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "# HybridValues"
      ]
    },
    {
      "cell_type": "markdown",
      "id": "license_cell",
      "metadata": {
        "tags": [
          "remove-cell"
        ]
      },
      "source": [
        "GTSAM Copyright 2010-2022, Georgia Tech Research Corporation,\nAtlanta, Georgia 30332-0415\nAll Rights Reserved\n\nAuthors: Frank Dellaert, et al. (see THANKS for the full author list)\n\nSee LICENSE for the license information"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/hybrid/doc/HybridValues.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 1,
      "metadata": {
        "tags": [
          "remove-cell"
        ]
      },
      "outputs": [],
      "source": [
        "try:\n",
        "    import google.colab\n",
        "    %pip install --quiet gtsam-develop\n",
        "except ImportError:\n",
        "    pass  # Not running on Colab, do nothing"
      ]
    },
    {
      "cell_type": "markdown",
      "id": "9c1d5e4c",
      "metadata": {},
      "source": [
        "`HybridValues` is a container class in GTSAM designed to hold results from hybrid inference. It stores three types of variable assignments simultaneously:\n",
        "\n",
        "1.  **Continuous `VectorValues`**: Stores vector-valued assignments for continuous variables, typically used with Gaussian factors/conditionals (`gtsam.GaussianFactor`, `gtsam.GaussianConditional`). Keys are often denoted with `V(index)`.\n",
        "2.  **Discrete `DiscreteValues`**: Stores assignments (unsigned integers) for discrete variables (`gtsam.DiscreteKey`, `gtsam.DiscreteFactor`). Keys are often denoted with `D(index)`.\n",
        "3.  **Nonlinear `Values`**: Stores assignments for variables living on manifolds, used with nonlinear factors (`gtsam.NonlinearFactor`). Keys are often denoted with `M(index)` (or other symbols like `X` if more generic).\n",
        "\n",
        "It provides a unified way to represent the complete state (or solution) in a hybrid system."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 2,
      "id": "f649cde1",
      "metadata": {},
      "outputs": [],
      "source": [
        "import gtsam\n",
        "import numpy as np\n",
        "\n",
        "from gtsam import HybridValues, VectorValues, DiscreteValues, Values, Pose2\n",
        "from gtsam.symbol_shorthand import V, D, M  # Use V, D, M for keys"
      ]
    },
    {
      "cell_type": "markdown",
      "id": "1f06af85",
      "metadata": {},
      "source": [
        "## Initialization"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 3,
      "id": "2d0c69ee",
      "metadata": {},
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "Empty HybridValues:\n",
            "HybridValues: \n",
            "  Continuous: 0 elements\n",
            "  Discrete: \n",
            "  Nonlinear\n",
            "Values with 0 values:\n",
            "\n",
            "HybridValues from VectorValues and DiscreteValues:\n",
            "HybridValues: \n",
            "  Continuous: 2 elements\n",
            "  v0: 1 2\n",
            "  v1: 3\n",
            "  Discrete: (d0, 1)(d1, 0)\n",
            "  Nonlinear\n",
            "Values with 0 values:\n",
            "\n",
            "HybridValues from all three types:\n",
            "HybridValues: \n",
            "  Continuous: 2 elements\n",
            "  v0: 1 2\n",
            "  v1: 3\n",
            "  Discrete: (d0, 1)(d1, 0)\n",
            "  Nonlinear\n",
            "Values with 1 values:\n",
            "Value m5: (gtsam::Pose2)\n",
            "(1, 2, 0.3)\n",
            "\n"
          ]
        }
      ],
      "source": [
        "# 1. Default constructor (empty)\n",
        "hybrid_values_empty = HybridValues()\n",
        "print(\"Empty HybridValues:\")\n",
        "hybrid_values_empty.print()\n",
        "\n",
        "# 2. From VectorValues and DiscreteValues\n",
        "vec_vals = VectorValues()\n",
        "vec_vals.insert(V(0), np.array([1.0, 2.0]))\n",
        "vec_vals.insert(V(1), np.array([3.0]))\n",
        "\n",
        "disc_vals = DiscreteValues()\n",
        "disc_vals[D(0)] = 1\n",
        "disc_vals[D(1)] = 0\n",
        "\n",
        "hybrid_values_vd = HybridValues(vec_vals, disc_vals)\n",
        "print(\"\\nHybridValues from VectorValues and DiscreteValues:\")\n",
        "hybrid_values_vd.print()\n",
        "\n",
        "# 3. From VectorValues, DiscreteValues, and Nonlinear Values\n",
        "nonlinear_vals = Values()\n",
        "nonlinear_vals.insert(M(5), Pose2(1, 2, 0.3)) # Example nonlinear type\n",
        "\n",
        "hybrid_values_all = HybridValues(vec_vals, disc_vals, nonlinear_vals)\n",
        "print(\"\\nHybridValues from all three types:\")\n",
        "hybrid_values_all.print()"
      ]
    },
    {
      "cell_type": "markdown",
      "id": "3c78f991",
      "metadata": {},
      "source": [
        "## Accessing Values\n",
        "\n",
        "Methods are provided to access the underlying containers and check for key existence."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 4,
      "id": "0a5e1fbe",
      "metadata": {},
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "\n",
            "Accessed Continuous Values size: 2\n",
            "Accessed Continuous Values:: 2 elements\n",
            "  v0: 1 2\n",
            "  v1: 3\n",
            "Accessed Discrete Values size: 2\n",
            "Accessed Nonlinear Values size: 1\n",
            "Accessed Nonlinear Values:\n",
            "Values with 1 values:\n",
            "Value m5: (gtsam::Pose2)\n",
            "(1, 2, 0.3)\n",
            "Accessed Discrete Values:: (d0, 1)\n",
            "\n",
            "Exists Vector V(0)? True\n",
            "Exists Discrete D(1)? True\n",
            "Exists Nonlinear M(5)? True\n",
            "Exists Vector D(0)? False\n",
            "Exists V(0) (any type)? True\n",
            "Exists D(0) (any type)? True\n",
            "Exists M(5) (any type)? True\n",
            "\n",
            "Value at V(0): [1. 2.]\n",
            "Value at D(0) (via atDiscrete): 1\n",
            "Value at D(0) (via discrete() dict access): 1\n",
            "Value at M(5): (1, 2, 0.3)\n",
            "\n",
            "(d1, 0)\n"
          ]
        }
      ],
      "source": [
        "# Access underlying containers\n",
        "cont_vals = hybrid_values_all.continuous()\n",
        "disc_vals_acc = hybrid_values_all.discrete()\n",
        "nonlin_vals_acc = hybrid_values_all.nonlinear()\n",
        "\n",
        "print(f\"\\nAccessed Continuous Values size: {cont_vals.size()}\")\n",
        "cont_vals.print(\"Accessed Continuous Values:\")\n",
        "print(f\"Accessed Discrete Values size: {len(disc_vals_acc)}\") # DiscreteValues acts like dict\n",
        "gtsam.PrintDiscreteValues(disc_vals_acc,\"Accessed Discrete Values:\")\n",
        "print(f\"Accessed Nonlinear Values size: {nonlin_vals_acc.size()}\")\n",
        "nonlin_vals_acc.print(\"Accessed Nonlinear Values:\")\n",
        "\n",
        "# Check existence\n",
        "print(f\"\\nExists Vector V(0)? {hybrid_values_all.existsVector(V(0))}\")\n",
        "print(f\"Exists Discrete D(1)? {hybrid_values_all.existsDiscrete(D(1))}\")\n",
        "print(f\"Exists Nonlinear M(5)? {hybrid_values_all.existsNonlinear(M(5))}\")\n",
        "print(f\"Exists Vector D(0)? {hybrid_values_all.existsVector(D(0))}\") # False, D(0) is a discrete key\n",
        "\n",
        "# exists() checks across all types (nonlinear, then vector, then discrete)\n",
        "print(f\"Exists V(0) (any type)? {hybrid_values_all.exists(V(0))}\") # Checks VectorValues\n",
        "print(f\"Exists D(0) (any type)? {hybrid_values_all.exists(D(0))}\") # Checks DiscreteValues\n",
        "print(f\"Exists M(5) (any type)? {hybrid_values_all.exists(M(5))}\") # Checks Nonlinear Values\n",
        "\n",
        "# Access specific values\n",
        "print(f\"\\nValue at V(0): {hybrid_values_all.at(V(0))}\")\n",
        "print(f\"Value at D(0) (via atDiscrete): {hybrid_values_all.atDiscrete(D(0))}\") \n",
        "print(f\"Value at D(0) (via discrete() dict access): {hybrid_values_all.discrete()[D(0)]}\")\n",
        "print(f\"Value at M(5): {hybrid_values_all.nonlinear().atPose2(M(5))}\") # Use type-specific getter from nonlinear() part"
      ]
    },
    {
      "cell_type": "markdown",
      "id": "2f4c779c",
      "metadata": {},
      "source": [
        "## Modifying Values (Insert, Update, Retract)\n",
        "\n",
        "Values can be inserted individually or from other containers. `update` modifies existing keys if they exist, while `insert` typically adds new keys (or might error/overwrite for some specific insert methods if key already exists). `insert_or_assign` will update if the key exists, or insert if it's new. `retract` applies a delta primarily to the `nonlinearValues_` part."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 5,
      "id": "25c0a8da",
      "metadata": {},
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "After individual inserts:\n",
            "HybridValues: \n",
            "  Continuous: 1 elements\n",
            "  v10: 1 2\n",
            "  Discrete: (d10, 1)\n",
            "  Nonlinear\n",
            "Values with 1 values:\n",
            "Value m11: (gtsam::Pose2)\n",
            "(0.1, 0.2, 0.03)\n",
            "\n",
            "\n",
            "After container inserts:\n",
            "HybridValues: \n",
            "  Continuous: 2 elements\n",
            "  v10: 1 2\n",
            "  v12: 5\n",
            "  Discrete: (d10, 1)(d11, 0)\n",
            "  Nonlinear\n",
            "Values with 2 values:\n",
            "Value m11: (gtsam::Pose2)\n",
            "(0.1, 0.2, 0.03)\n",
            "\n",
            "Value m15: (gtsam::Pose2)\n",
            "(1, 1, 0)\n",
            "\n"
          ]
        }
      ],
      "source": [
        "hv = HybridValues()\n",
        "\n",
        "# Insert individual values\n",
        "hv.insert(V(10), np.array([1.0, 2.0])) # Vector value, goes into hv.continuous()\n",
        "hv.insert(D(10), 1)                    # Discrete value, goes into hv.discrete()\n",
        "hv.insertNonlinear(M(11), Pose2(0.1, 0.2, 0.03)) # Nonlinear value, goes into hv.nonlinear()\n",
        "\n",
        "print(\"After individual inserts:\")\n",
        "hv.print()\n",
        "\n",
        "# Insert from containers\n",
        "new_vec = VectorValues()\n",
        "new_vec.insert(V(12), np.array([5.0]))\n",
        "new_disc = DiscreteValues()\n",
        "new_disc[D(11)] = 0\n",
        "new_nonlin = Values()\n",
        "new_nonlin.insert(M(15), Pose2(1,1,0))\n",
        "\n",
        "hv.insert(new_vec)      # Merges into hv.continuous()\n",
        "hv.insert(new_disc)     # Merges into hv.discrete()\n",
        "hv.insert(new_nonlin)   # Merges into hv.nonlinear()\n",
        "print(\"\\nAfter container inserts:\")\n",
        "hv.print()"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 6,
      "id": "00eeee4e",
      "metadata": {},
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "\n",
            "After update:\n",
            "HybridValues: \n",
            "  Continuous: 2 elements\n",
            "  v10: 99 98\n",
            "  v12: 5\n",
            "  Discrete: (d10, 2)(d11, 0)\n",
            "  Nonlinear\n",
            "Values with 2 values:\n",
            "Value m11: (gtsam::Pose2)\n",
            "(0.5, 0.6, 0.07)\n",
            "\n",
            "Value m15: (gtsam::Pose2)\n",
            "(1, 1, 0)\n",
            "\n"
          ]
        }
      ],
      "source": [
        "# Update existing values\n",
        "update_vec = VectorValues()\n",
        "update_vec.insert(V(10), np.array([99.0, 98.0]))\n",
        "update_disc = DiscreteValues()\n",
        "update_disc[D(10)] = 2\n",
        "update_nonlin = Values()\n",
        "update_nonlin.insert(M(11), Pose2(0.5,0.6,0.07))\n",
        "\n",
        "hv.update(update_vec)\n",
        "hv.update(update_disc)\n",
        "hv.update(update_nonlin)\n",
        "print(\"\\nAfter update:\")\n",
        "hv.print()"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 7,
      "id": "e5b10c13",
      "metadata": {},
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "\n",
            "After retract (M(11) should change, V(10) should not):\n",
            "HybridValues: \n",
            "  Continuous: 2 elements\n",
            "  v10: 99 98\n",
            "  v12: 5\n",
            "  Discrete: (d10, 2)(d11, 0)\n",
            "  Nonlinear\n",
            "Values with 2 values:\n",
            "Value m11: (gtsam::Pose2)\n",
            "(0.553375, 0.55362, 0.08)\n",
            "\n",
            "Value m15: (gtsam::Pose2)\n",
            "(1, 1, 0)\n",
            "\n"
          ]
        }
      ],
      "source": [
        "# Retract (applies delta to the nonlinearValues_ part of HybridValues)\n",
        "# Note: The continuous_ (VectorValues) part is NOT retracted by HybridValues.retract itself.\n",
        "delta_nl = VectorValues() # Deltas are always VectorValues\n",
        "# Create a delta for the Pose2 at M(11). M(11) current value: Pose2(0.5,0.6,0.07)\n",
        "delta_pose2_M11 = np.array([0.05, -0.05, 0.01]) # dx, dy, dtheta\n",
        "delta_nl.insert(M(11), delta_pose2_M11)\n",
        "\n",
        "hv_retracted = hv.retract(delta_nl)\n",
        "print(\"\\nAfter retract (M(11) should change, V(10) should not):\")\n",
        "hv_retracted.print()"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 8,
      "id": "263a8269",
      "metadata": {},
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "\n",
            "After insert_or_assign (for V and D keys):\n",
            "HybridValues: \n",
            "  Continuous: 3 elements\n",
            "  v10: 100 101\n",
            "  v12: 5\n",
            "  v13: 13\n",
            "  Discrete: (d10, 2)(d11, 0)(d12, 1)\n",
            "  Nonlinear\n",
            "Values with 2 values:\n",
            "Value m11: (gtsam::Pose2)\n",
            "(0.5, 0.6, 0.07)\n",
            "\n",
            "Value m15: (gtsam::Pose2)\n",
            "(1, 1, 0)\n",
            "\n"
          ]
        }
      ],
      "source": [
        "# Insert or assign\n",
        "# Replaces if exists, inserts if not.\n",
        "hv.insert_or_assign(V(10), np.array([100.0, 101.0])) # Overwrites V(10) in continuous_\n",
        "hv.insert_or_assign(D(12), 1) # Inserts D(12) in discrete_\n",
        "hv.insert_or_assign(V(13), np.array([13.0])) # Inserts V(13) in continuous_\n",
        "# Note: insert_or_assign for nonlinear types is not directly on HybridValues.\n",
        "# You would typically do this on the underlying Values container:\n",
        "# hv.nonlinear().insert_or_assign(M(11), Pose2(...)) # if Values had insert_or_assign\n",
        "# Or, more commonly, for nonlinear: hv.nonlinear().update(M(11), Pose2(...))\n",
        "print(\"\\nAfter insert_or_assign (for V and D keys):\")\n",
        "hv.print()"
      ]
    }
  ],
  "metadata": {
    "kernelspec": {
      "display_name": "py312",
      "language": "python",
      "name": "python3"
    },
    "language_info": {
      "codemirror_mode": {
        "name": "ipython",
        "version": 3
      },
      "file_extension": ".py",
      "mimetype": "text/x-python",
      "name": "python",
      "nbconvert_exporter": "python",
      "pygments_lexer": "ipython3",
      "version": "3.12.6"
    }
  },
  "nbformat": 4,
  "nbformat_minor": 5
}
